# The MIT License (MIT)
#
# Copyright (c) 2021 , 01Studio
#
# version: v1.1 
#
# date: 2022-01-20

from machine import Pin,PWM,SoftI2C
from ssd1306 import SSD1306_I2C     #从ssd1306模块中导入SSD1306_I2C子模块
import framebuf
import time

#编码盘计数
count1 = 0
count2 = 0

class CAR():
    
    #初始化
    def __init__(self):
        
        #电机1
        self.M1_P = PWM(Pin(14), freq=1000, duty=0) 
        self.M1_N = PWM(Pin(15), freq=1000, duty=0)
        
        #电机2
        self.M2_P = PWM(Pin(16), freq=1000, duty=0) 
        self.M2_N = PWM(Pin(17), freq=1000, duty=0)
        
        #电机3
        self.M3_P = PWM(Pin(18), freq=1000, duty=0) 
        self.M3_N = PWM(Pin(19), freq=1000, duty=0)

        #电机4
        self.M4_P = PWM(Pin(21), freq=1000, duty=0) 
        self.M4_N = PWM(Pin(22), freq=1000, duty=0)

        #电机速度，默认最大
        self.duty=1023
        
        #车灯      
        self.light = Pin(5, Pin.OUT)  # 车头灯
        
        #尾灯（两个红色LED）
        self.tail_light_left = Pin(0, Pin.OUT)   # 左尾灯
        self.tail_light_right = Pin(12, Pin.OUT) # 右尾灯

        #超声波测距
        self.trig = Pin(27,Pin.OUT)
        self.echo = Pin(26,Pin.IN)
        
        #编码盘测速
        self.speed = Pin(4,Pin.IN,Pin.PULL_UP)
        self.speed.irq(self.speed_count,Pin.IRQ_FALLING)
        self.speed2 = Pin(13,Pin.IN,Pin.PULL_UP)
        self.speed2.irq(self.speed_count2,Pin.IRQ_FALLING)
        
        #巡线传感器初始化，五路光电
        self.t1 = Pin(33, Pin.IN, Pin.PULL_UP)
        self.t2 = Pin(34, Pin.IN, Pin.PULL_UP)
        self.t3 = Pin(35, Pin.IN, Pin.PULL_UP)
        self.t4 = Pin(36, Pin.IN, Pin.PULL_UP)
        self.t5 = Pin(39, Pin.IN, Pin.PULL_UP)
        
        #红外接收头
        self.IR = Pin(32, Pin.IN)
        
        #屏幕
        self.i2c=SoftI2C(sda=Pin(25), scl=Pin(23))
        self.oled=SSD1306_I2C(128, 64,self.i2c,addr=0x3c)

        self.oled.text("Hal3000", 0, 0, 1)
        self.oled.show()
        time.sleep(2)
        self.s_wifi=0
        self.s_blue=0
        self.s_red=0
        self.s_light=0        # 车头灯状态
        self.s_tail_left=0    # 左尾灯状态
        self.s_tail_right=0   # 右尾灯状态
        self.s_forward=0
        self.s_backward=0
        self.s_left=0
        self.s_right=0
        self.s_sr=0
        self.s_distance=0
        self.screen()

    #屏幕刷新    
    def screen(self):
        if self.i2c.scan()[0]==0x3c:
            self.oled.fill(0)
            wifibuf=framebuf.FrameBuffer(bytearray(b'\x00\x00\x00\x00\x10\x08\x14(\x14(\x14(\x10\x08\x00\x00\x01\x80\x01\x80\x01\x80\x01\xc0\x03\xc0\x00\x00\x00\x00\x00\x00'),16,16,framebuf.MONO_HLSB)
            bluetoothbuf=framebuf.FrameBuffer(bytearray(b'\x00\x00\x00\x00\x01\x80\x01\xc0\x05`\x07\xc0\x03\x80\x01\x80\x03\xc0\x05`\x01\xc0\x01\x80\x00\x00\x00\x00\x00\x00\x00\x00'),16,16,framebuf.MONO_HLSB)
            redbuf=framebuf.FrameBuffer(bytearray(b'\x00\x00\x00\x00\x00\x00\x00\x00\x01\x80\x19\x98\t\x90\x05\xa0\x04 \x00\x00\x03\xc0\x07\xe0\x1f\xf8\x00\x00\x00\x00\x00\x00'),16,16,framebuf.MONO_HLSB)
            lightbuf=framebuf.FrameBuffer(bytearray(b'\x00\x00\x03\x80\x0c`\x080\x10\x10\x10\x10\x11\x10\x19\x10\t \x05`\x07\xc0\x04@\x04@\x07\xc0\x03\x80\x00\x00'),16,16,framebuf.MONO_HLSB)
            arrowbuf=framebuf.FrameBuffer(bytearray(b'\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\xc0\x00\x00\x00\x00\x00\x00\x03\xe0\x00\x00\x00\x00\x00\x00\x07\xf0\x00\x00\x00\x00\x00\x00\x0f\xf8\x00\x00\x00\x00\x00\x00\x0f\xf8\x00\x00\x00\x00\x00\x00\x1e<\x00\x00\x00\x00\x00\x00<\x1e\x00\x00\x00\x00\x00\x00x\x0f\x00\x00\x00\x00\x00\x00x\x0f\x00\x00\x00\x00\x00\x00\xf0\x07\x80\x00\x00\x00\x00\x01\xe0\x03\xc0\x00\x00\x00\x00\x01\xe0\x03\xc0\x00\x00\x00\x00\x01\xfc\x1f\xc0\x00\x00\x00\x00\x01\xfc\x1f\xc0\x00\x00\x00\x00\x00\xfc\x1f\x80\x00\x00\x00\x00\x00\x1c\x1c\x00\x00\x00\x00\x00\x00\x1c\x1c\x00\x00\x00\x00\x00\x00\x1c\x1c\x00\x00\x00\x00\x00\x00\x1c\x1c\x00\x00\x00\x00\x00\x00\x1c\x1c\x00\x00\x00\x00\x00\x00\x1c\x1c\x00\x00\x00\x00\x0f\x00\x1c\x1c\x00x\x00\x00\x1f\x80\x1c\x1c\x00\xfc\x00\x00\x7f\x80\x1c\x1c\x00\xff\x00\x00\xff\x80\x1c\x1c\x00\xff\x80\x01\xf3\xff\xff\xff\xff\xe7\xc0\x07\xe3\xff\xff\xff\xff\xe3\xf0\x0f\x83\xff\xff\xff\xff\xe0\xf8\x1f\x00\x00\x1f\xfc\x00\x00|>\x00\x00\x1f\xfc\x00\x00>>\x00\x00\x1f\xfc\x00\x00>>\x00\x00\x1f\xfc\x00\x00>\x1f\x00\x00\x1f\xfc\x00\x00|\x0f\x83\xff\xff\xff\xff\xe0\xf8\x07\xe3\xff\xff\xff\xff\xe3\xf0\x01\xf3\xff\xff\xff\xff\xe7\xc0\x00\xff\x80\x1c\x1c\x00\xff\x80\x00\x7f\x80\x1c\x1c\x00\xff\x00\x00\x1f\x80\x1c\x1c\x00\xfc\x00\x00\x0f\x00\x1c\x1c\x00x\x00\x00\x00\x00\x1c\x1c\x00\x00\x00\x00\x00\x00\x1c\x1c\x00\x00\x00\x00\x00\x00\x1c\x1c\x00\x00\x00\x00\x00\x00\x1c\x1c\x00\x00\x00\x00\x00\x00\x1c\x1c\x00\x00\x00\x00\x00\x00\x1c\x1c\x00\x00\x00\x00\x00\x00\xfc\x1f\x80\x00\x00\x00\x00\x01\xfc\x1f\xc0\x00\x00\x00\x00\x01\xfc\x1f\xc0\x00\x00\x00\x00\x01\xe0\x03\xc0\x00\x00\x00\x00\x01\xe0\x03\xc0\x00\x00\x00\x00\x00\xf0\x07\x80\x00\x00\x00\x00\x00x\x0f\x00\x00\x00\x00\x00\x00x\x0f\x00\x00\x00\x00\x00\x00<\x1e\x00\x00\x00\x00\x00\x00\x1e<\x00\x00\x00\x00\x00\x00\x0f\xf8\x00\x00\x00\x00\x00\x00\x0f\xf8\x00\x00\x00\x00\x00\x00\x07\xf0\x00\x00\x00\x00\x00\x00\x03\xe0\x00\x00\x00\x00\x00\x00\x01\xc0\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),64,64,framebuf.MONO_HLSB)
            
            if self.s_wifi:
                self.oled.blit(wifibuf,0,1)
            if self.s_blue:
                self.oled.blit(bluetoothbuf,0,17)
            if self.s_red:
                self.oled.blit(redbuf,0,31)
            if self.s_light:
                self.oled.blit(lightbuf,1,47)
                
            self.oled.blit(arrowbuf,17,0)
            
            if self.s_forward:
                self.oled.fill_rect(49,27,26,11,1)
            if self.s_backward:
                self.oled.fill_rect(24,27,26,11,1)
            if self.s_left:
                self.oled.fill_rect(44,7,11,26,1)
            if self.s_right:
                self.oled.fill_rect(44,32,11,26,1)
            
            self.oled.text("D:cm", 84,8)
            self.oled.text("%.2f" % self.s_distance,84,18)
            self.oled.text("G:cm",84,38)   
            self.oled.text("%.2f" % self.s_sr,84,48)      
            self.oled.vline(16,0,64,1)
            self.oled.vline(82,0,64,1)
            self.oled.hline(82,32,40,1)
            self.oled.rect(0,0,128,64,1)

            self.oled.show()
        else:
            print('screen connect error!')
        
    #调速
    def setspeed(self,speed):
        self.duty=speed
        if self.s_forward==1:
            self.forward()
        if self.s_backward==1:
            self.backward()
        if self.s_left==1:
            self.turn_left()
        if self.s_right==1:
            self.turn_right()
    #前进    
    def forward(self):
        speed=self.duty
        self.s_forward=1
        self.s_backward=0
        self.s_left=0
        self.s_right=0
        
        self.M1_P.duty(speed)
        self.M1_N.duty(0)    
        self.M2_P.duty(speed)
        self.M2_N.duty(0)
        self.M3_P.duty(speed)
        self.M3_N.duty(0)
        self.M4_P.duty(speed)
        self.M4_N.duty(0)
    
    #后退
    def backward(self):
        speed=self.duty
        self.s_forward=0
        self.s_backward=1
        self.s_left=0
        self.s_right=0

        self.M1_P.duty(0)
        self.M1_N.duty(speed)    
        self.M2_P.duty(0)
        self.M2_N.duty(speed)
        self.M3_P.duty(0)
        self.M3_N.duty(speed)
        self.M4_P.duty(0)
        self.M4_N.duty(speed)

    #左转
    def turn_left(self, mode=0):
        speed=self.duty
        self.s_forward=0
        self.s_backward=0
        self.s_left=1
        self.s_right=0
        
        #普通转向
        if mode == 0: 
            self.M1_P.duty(0)
            self.M1_N.duty(0)    
            self.M2_P.duty(speed)
            self.M2_N.duty(0)
            self.M3_P.duty(speed)
            self.M3_N.duty(0)
            self.M4_P.duty(0)
            self.M4_N.duty(0)
        
        #大幅度转向
        elif mode ==1:
            self.M1_P.duty(0)
            self.M1_N.duty(speed)    
            self.M2_P.duty(speed)
            self.M2_N.duty(0)
            self.M3_P.duty(speed)
            self.M3_N.duty(0)
            self.M4_P.duty(0)
            self.M4_N.duty(speed)

    #右转
    def turn_right(self, mode=0):
        speed=self.duty
        self.s_forward=0
        self.s_backward=0
        self.s_left=0
        self.s_right=1
        #普通转向
        if mode == 0:
            self.M1_P.duty(speed)
            self.M1_N.duty(0)    
            self.M2_P.duty(0)
            self.M2_N.duty(0)
            self.M3_P.duty(0)
            self.M3_N.duty(0)
            self.M4_P.duty(speed)
            self.M4_N.duty(0)
        #大幅度转向
        elif mode == 1: 
            self.M1_P.duty(speed)
            self.M1_N.duty(0)    
            self.M2_P.duty(0)
            self.M2_N.duty(speed)
            self.M3_P.duty(0)
            self.M3_N.duty(speed)
            self.M4_P.duty(speed)
            self.M4_N.duty(0)

    #停止
    def stop(self):
        self.s_forward=0
        self.s_backward=0
        self.s_left=0
        self.s_right=0
        self.M1_P.duty(0)
        self.M1_N.duty(0)    
        self.M2_P.duty(0)
        self.M2_N.duty(0)
        self.M3_P.duty(0)
        self.M3_N.duty(0)
        self.M4_P.duty(0)
        self.M4_N.duty(0)

    #按时间控制前进距离 (单位：厘米)
    def forward_distance(self, distance=20):
        """使用时间控制前进指定距离，默认20厘米"""
        estimated_speed = 15  # cm/s，根据实际测试调整
        forward_time = distance / estimated_speed
        
        print("前进", distance, "cm")
        
        # 开始前进
        speed = self.duty
        self.s_forward = 1
        self.s_backward = 0
        self.s_left = 0
        self.s_right = 0
        
        self.M1_P.duty(speed)
        self.M1_N.duty(0)    
        self.M2_P.duty(speed)
        self.M2_N.duty(0)
        self.M3_P.duty(speed)
        self.M3_N.duty(0)
        self.M4_P.duty(speed)
        self.M4_N.duty(0)
        
        # 按时间运行
        time.sleep(forward_time)
        
        # 停止
        self.stop()
        print("完成")

    #速度校准功能
    def calibrate_speed(self, test_time=3.0):
        """速度校准：让小车运行指定时间，测量实际距离来校准速度参数"""
        print("校准开始,前进", test_time, "秒")
        
        # 等待3秒让用户准备
        for i in range(3, 0, -1):
            print(i)
            time.sleep(1)
        
        print("开始!")
        
        # 开始前进
        speed = self.duty
        self.s_forward = 1
        self.s_backward = 0
        self.s_left = 0
        self.s_right = 0
        
        self.M1_P.duty(speed)
        self.M1_N.duty(0)    
        self.M2_P.duty(speed)
        self.M2_N.duty(0)
        self.M3_P.duty(speed)
        self.M3_N.duty(0)
        self.M4_P.duty(speed)
        self.M4_N.duty(0)
        
        # 按时间运行
        time.sleep(test_time)
        
        # 停止
        self.stop()
        
        print("停止! 请测量距离")
        print("计算公式: 距离÷", test_time, "=速度cm/s")



    #####################
    # Mecanum麦克纳母轮
    #####################   
    def up(self): #上移
        
        speed=self.duty        
        self.M1_P.duty(speed)
        self.M1_N.duty(0)    
        self.M2_P.duty(speed)
        self.M2_N.duty(0)
        self.M3_P.duty(speed)
        self.M3_N.duty(0)
        self.M4_P.duty(speed)
        self.M4_N.duty(0)
        
    def down(self): #下移
        
        speed=self.duty 
        self.M1_P.duty(0)
        self.M1_N.duty(speed)    
        self.M2_P.duty(0)
        self.M2_N.duty(speed)
        self.M3_P.duty(0)
        self.M3_N.duty(speed)
        self.M4_P.duty(0)
        self.M4_N.duty(speed)

    def left(self): #左移
        
        speed=self.duty 
        self.M1_P.duty(0)
        self.M1_N.duty(speed)    
        self.M2_P.duty(speed)
        self.M2_N.duty(0)
        self.M3_P.duty(0)
        self.M3_N.duty(speed)
        self.M4_P.duty(speed)
        self.M4_N.duty(0)

    def right(self): #右移
        
        speed=self.duty 
        self.M1_P.duty(speed)
        self.M1_N.duty(0)    
        self.M2_P.duty(0)
        self.M2_N.duty(speed)
        self.M3_P.duty(speed)
        self.M3_N.duty(0)
        self.M4_P.duty(0)
        self.M4_N.duty(speed)

    def up_left(self): #左前移
        
        speed=self.duty 
        self.M1_P.duty(0)
        self.M1_N.duty(0)    
        self.M2_P.duty(speed)
        self.M2_N.duty(0)
        self.M3_P.duty(0)
        self.M3_N.duty(0)
        self.M4_P.duty(speed)
        self.M4_N.duty(0)

    def up_right(self): #右前移
        
        speed=self.duty 
        self.M1_P.duty(speed)
        self.M1_N.duty(0)    
        self.M2_P.duty(0)
        self.M2_N.duty(0)
        self.M3_P.duty(speed)
        self.M3_N.duty(0)
        self.M4_P.duty(0)
        self.M4_N.duty(0)

    def down_left(self): #左后移
        
        speed=self.duty 
        self.M1_P.duty(0)
        self.M1_N.duty(speed)    
        self.M2_P.duty(0)
        self.M2_N.duty(0)
        self.M3_P.duty(0)
        self.M3_N.duty(speed)
        self.M4_P.duty(0)
        self.M4_N.duty(0)
    
    def down_right(self): #右后移
        
        speed=self.duty 
        self.M1_P.duty(0)
        self.M1_N.duty(0)    
        self.M2_P.duty(0)
        self.M2_N.duty(speed)
        self.M3_P.duty(0)
        self.M3_N.duty(0)
        self.M4_P.duty(0)
        self.M4_N.duty(speed)

    def clockwise(self): #顺时针旋转
        
        speed=self.duty 
        self.M1_P.duty(speed)
        self.M1_N.duty(0)    
        self.M2_P.duty(0)
        self.M2_N.duty(speed)
        self.M3_P.duty(0)
        self.M3_N.duty(speed)
        self.M4_P.duty(speed)
        self.M4_N.duty(0)

    def Counterclockwise(self): #逆时针旋转
        
        speed=self.duty 
        self.M1_P.duty(0)
        self.M1_N.duty(speed)    
        self.M2_P.duty(speed)
        self.M2_N.duty(0)
        self.M3_P.duty(speed)
        self.M3_N.duty(0)
        self.M4_P.duty(0)
        self.M4_N.duty(speed)
    

    #打开所有车灯（车头灯+尾灯）
    def light_on(self):
        
        self.light.on()           # 车头灯
        self.tail_light_left.on() # 左尾灯
        self.tail_light_right.on() # 右尾灯
        self.s_light=1
        self.s_tail_left=1
        self.s_tail_right=1
    
    #关闭所有车灯
    def light_off(self):
        
        self.light.off()           # 车头灯
        self.tail_light_left.off() # 左尾灯  
        self.tail_light_right.off() # 右尾灯
        self.s_light=0
        self.s_tail_left=0
        self.s_tail_right=0
    
    #所有车灯引脚输出
    def light(self,value):
        
        if value == 0:
            self.light_off()
        elif value == 1:
            self.light_on()

    #只控制尾灯
    def tail_lights_on(self):
        """只开启尾灯"""
        self.tail_light_left.on()
        self.tail_light_right.on()
        self.s_tail_left=1
        self.s_tail_right=1
        print("尾灯开")

    def tail_lights_off(self):
        """只关闭尾灯"""
        self.tail_light_left.off()
        self.tail_light_right.off()
        self.s_tail_left=0
        self.s_tail_right=0
        print("尾灯关")

    def tail_lights_toggle(self):
        """切换尾灯状态"""
        if self.s_tail_left == 1:
            self.tail_lights_off()
        else:
            self.tail_lights_on()

    #所有灯光切换 (开关所有灯)
    def light_toggle(self):
        """切换所有灯光状态：开→关，关→开"""
        if self.s_light == 1:  # 当前开着
            self.light_off()
            print("关灯")
        else:  # 当前关着
            self.light_on()
            print("开灯")

    #所有灯光闪烁（车头灯+尾灯一起）
    def light_blink(self, times=5, interval=0.3):
        """所有灯光闪烁指定次数"""
        print("闪烁", times, "次")
        # 记录原始状态
        original_light = self.s_light
        original_tail_left = self.s_tail_left
        original_tail_right = self.s_tail_right
        
        for i in range(times):
            # 所有灯一起开
            self.light.on()
            self.tail_light_left.on()
            self.tail_light_right.on()
            time.sleep(interval)
            
            # 所有灯一起关
            self.light.off()
            self.tail_light_left.off() 
            self.tail_light_right.off()
            time.sleep(interval)
        
        # 恢复原始状态
        if original_light == 1:
            self.light.on()
            self.s_light = 1
        else:
            self.light.off()
            self.s_light = 0
            
        if original_tail_left == 1:
            self.tail_light_left.on()
            self.s_tail_left = 1
        else:
            self.tail_light_left.off()
            self.s_tail_left = 0
            
        if original_tail_right == 1:
            self.tail_light_right.on()
            self.s_tail_right = 1
        else:
            self.tail_light_right.off()
            self.s_tail_right = 0
            
        print("闪烁完成")

    #顺时针旋转一圈
    def rotate_clockwise_full(self, rotation_time=3.0):
        """顺时针旋转一圈"""
        print("顺时针旋转")
        self.clockwise()
        time.sleep(rotation_time)
        self.stop()
        print("旋转完成")

    #逆时针旋转一圈
    def rotate_counterclockwise_full(self, rotation_time=3.0):
        """逆时针旋转一圈"""
        print("逆时针旋转")
        self.Counterclockwise()
        time.sleep(rotation_time)
        self.stop()
        print("旋转完成")

    #超声波测距
    def getDistance(self):
        distance=0
        self.trig.value(1)
        time.sleep_us(20)
        self.trig.value(0)
        while self.echo.value() == 0:
            pass
        if self.echo.value() == 1:
            ts=time.ticks_us()            #开始时间
            while self.echo.value() == 1: #等待脉冲高电平结束
                pass
            te=time.ticks_us()            #结束时间
            tc=te-ts                      #回响时间（单位us，1us=1*10^(-6)s）
            distance=(tc*170)/10000       #距离计算 （单位为:cm）
        self.s_sr=distance
            
        return distance

    #编码盘测速计数(1为左，2为右)
    def speed_count(self,args):
        
        global count1
        count1 = count1+1
        
    def speed_count2(self,args):
        
        global count2
        count2 = count2+1

    #返回行驶路程 
    def getJourney(self):     
        
        global count1
        global count2
        self.s_distance=count1/20*0.188
        return '%.2f'%(count1/20*0.188),'%.2f'%(count2/20*0.188)
    
    #路程清0
    def journey_clear(self):       
        
        global count1
        global count2
        count1 = 0
        count2 = 0

    #五路光电传感器
    def T1(self):
        
        return self.t1.value()

    def T2(self):
        
        return self.t2.value()
    
    def T3(self):
        
        return self.t3.value()

    def T4(self):
        
        return self.t4.value()
    
    def T5(self):
        
        return self.t5.value()

    #红外接收解码
    def getIR(self):

        if (self.IR.value() == 0):
            count = 0
            while ((self.IR.value() == 0) and (count < 100)): #9ms
                count += 1
                time.sleep_us(100)
            if(count < 10):
                return None
            count = 0
            while ((self.IR.value() == 1) and (count < 50)): #4.5ms
                count += 1
                time.sleep_us(100)
                
            idx = 0
            cnt = 0
            data = [0,0,0,0]
            for i in range(0,32):
                count = 0
                while ((self.IR.value() == 0) and (count < 10)):    #0.56ms
                    count += 1
                    time.sleep_us(100)

                count = 0
                while ((self.IR.value() == 1) and (count < 20)):   #0: 0.56mx
                    count += 1                                #1: 1.69ms
                    time.sleep_us(100)

                if count > 7:
                    data[idx] |= 1<<cnt
                if cnt == 7:
                    cnt = 0
                    idx += 1
                else:
                    cnt += 1

            if data[0]+data[1] == 0xFF and data[2]+data[3] == 0xFF:  #check
                return data[2]
            else:
                return("REPEAT")

